// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited

//
// This header declares the MapMaker class
// MapMaker makes and maintains the Map struct
// Starting with stereo initialisation from a bunch of matches
// over keyframe insertion, continual bundle adjustment and 
// data-association refinement.
// MapMaker runs in its own thread, although some functions
// (notably stereo init) are called by the tracker and run in the 
// tracker's thread.

#ifndef __MAPMAKER_H
#define __MAPMAKER_H
#include <cvd/image.h>
#include <cvd/byte.h>
#include <cvd/thread.h>
#include <fstream>
#include <iostream>

#include "Map.h"
#include "KeyFrame.h"
#include "ATANCamera.h"
#include "PoseEstimator.h"
#include <queue>
#include "PointSet.h"
#include "RBF.h"
#include "MiniPatch.h"

struct MarkerMeasurements    // This struct is used for initial correspondences
{
  Vector<3> v3WorldPos;
  std::map<boost::shared_ptr<KeyFrame>, CVD::ImageRef> mMeasurements;
};

struct TriangularPatch
{
  inline TriangularPatch(double dAngle, double dArea, boost::shared_ptr<KeyFrame> pKF, Vector<2> &v2Vertex0, Vector<2> &v2Vertex1, Vector<2> &v2Vertex2, Vector<3> &v3Vertex0, Vector<3> &v3Vertex1, Vector<3> &v3Vertex2)
  {
    dViewAngle = dAngle;
    dViewArea = dArea;
    pKeyframe = pKF;
    v2ImageVertex0 = v2Vertex0;
    v2ImageVertex1 = v2Vertex1;
    v2ImageVertex2 = v2Vertex2;
    v3WorldVertex0 = v3Vertex0;
    v3WorldVertex1 = v3Vertex1;
    v3WorldVertex2 = v3Vertex2;
  }

  double dViewAngle;          // Angle between patch normal and Keyframe optical axis
  double dViewArea;          // Pixel area of the patch in this view
  boost::shared_ptr<KeyFrame> pKeyframe;        // Keyframe in which patch is observed
  Vector<2> v2ImageVertex0;   // First vertex of triangular patch
  Vector<2> v2ImageVertex1;   // Second vertex of triangular patch
  Vector<2> v2ImageVertex2;   // Third vertex of triangular patch
  Matrix<3,3> m3Homography;


  Vector<3> v3WorldVertex0;
  Vector<3> v3WorldVertex1;
  Vector<3> v3WorldVertex2;

  //! Allow sorting a list of Triangular Patches into descending order of viewing angle using std::sort.
  inline bool operator< (TriangularPatch const& other) const {
    return dViewAngle > other.dViewAngle;
  }
};

// MapMaker dervives from CVD::Thread, so everything in void run() is its own thread.
class MapMaker : protected CVD::Thread
{
public:
  MapMaker(const ATANCamera &cam, Scarf &s);
  ~MapMaker();
  
  Map* GetMap() { return mMap; };
  const SE3GaussianPDF& GetGfromF() { return mMap->GetGfromF(); };
  const SE3<>& GetFfromW() { return mMap->GetFfromW(); };

  // Make a map from scratch. Called by the tracker.
  bool InitFromKeyFrames(std::vector<boost::shared_ptr<KeyFrame> > &vKeyFrames, std::map<int, MarkerMeasurements> &mMarkerMeasurements, SE3<> &se3Pose);

  void AddKeyFrame(boost::shared_ptr<KeyFrame> k);   // Add a key-frame to the map. Called by the tracker.
  void RequestReset(bool bCompleteReset);   // Request that the we reset. Called by the tracker.
  bool ResetDone();      // Returns true if the has been done.
  int  QueueSize() { return mvpKeyFrameQueue.size(); }; // How many KFs in the queue waiting to be added?
  bool IsPaused() { return mbMapMakerPaused; };
  bool NeedNewKeyFrame(boost::shared_ptr<KeyFrame> kCurrent, double dMaxKFDistWiggleMult = 1.0);            // Is it a good camera pose to add another KeyFrame?
  bool IsDistanceToNearestKeyFrameExcessive(boost::shared_ptr<KeyFrame> kCurrent);  // Is the camera far away from the nearest KeyFrame (i.e. maybe lost?)
  bool IsDistanceToNearestKeyFrameExcessive(double dDist);  // Is the current position far away from the nearest KeyFrame
  void UpdateTrackerPosition(SE3<> se3CfromW);   // Tell the MapMaker the current tracker position.
  void ForceLocalMapUpdate(SE3<> se3CfromW);
  boost::shared_ptr<KeyFrame> NearestKeyFrameToTrackerPosition() { return mpNearestTrackerKF; }
  boost::shared_ptr<KeyFrame> MostRecentKeyFrame() { return mpMostRecentKF; }
  
protected:
  Map *mMap;               // The map
  std::vector<Map*> mvOldMaps;               // The map

  ATANCamera mCamera;      // Same as the tracker's camera: N.B. not a reference variable!
  Scarf &mScarf;   // Keypoint detector and descriptor
  PoseEstimator mPoseEstimator;
  virtual void run();      // The MapMaker thread code lives here

  // Map expansion functions:
  //void MergeMaps();
  void AddKeyFrameFromTopOfQueue();  
  void ThinCandidates(boost::shared_ptr<KeyFrame> k, int nLevel);
  int AddSomeMapPoints(boost::shared_ptr<KeyFrame> k, int nLevel);
  bool AddPointEpipolar(boost::shared_ptr<KeyFrame> kSrc, boost::shared_ptr<KeyFrame> kTarget, int nLevel, int nCandidate);
  // Returns point in ref frame B
  Vector<3> ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B);
  
  // Bundle adjustment functions:
  void BundleAdjust(Map *pMap, std::set<boost::shared_ptr<KeyFrame> >, std::set<boost::shared_ptr<KeyFrame> >, std::set<boost::shared_ptr<MapPoint> >, bool bRecent, bool bGlobalSE3, bool bGlobalSO3);
  void BundleAdjustRecent(Map *pMap, bool bGlobalSE3, bool bGlobalSO3);

  // Data association functions:
  int ReFindInSingleKeyFrame(boost::shared_ptr<KeyFrame> k);
  void ReFindFromFailureQueue();
  void ReFindNewlyMade();
  void ReFindAll();
  bool ReFind_Common(boost::shared_ptr<KeyFrame> k, boost::shared_ptr<MapPoint> &p);
  void SubPixelRefineMatches(boost::shared_ptr<KeyFrame> k, int nLevel);
  
  // General Maintenance/Utility:
  void UpdateLocalMap();
  void Reset();
  void HandleBadPoints();
  double DistToNearestKeyFrame(boost::shared_ptr<KeyFrame> kCurrent);
  double KeyFrameLinearDist(boost::shared_ptr<KeyFrame> k1, boost::shared_ptr<KeyFrame> k2);
  double MaxBaseline(std::vector<boost::shared_ptr<KeyFrame> > &vKeyFrames, int &first, int &middle, int &last);
  std::vector<boost::shared_ptr<KeyFrame> > RTreeNClosestKeyFrames(boost::shared_ptr<KeyFrame> k, unsigned int N);
  boost::shared_ptr<KeyFrame> RTreeClosestKeyFrame(boost::shared_ptr<KeyFrame> k);
  boost::shared_ptr<KeyFrame> RTreeClosestKeyFrame(SE3<> se3CfromW);
  bool RefreshSceneDepth(boost::shared_ptr<KeyFrame> pKF);
  bool comp(float a, float b);

  // GUI Interface:
  void GUICommandHandler(std::string sCommand, std::string sParams);
  static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams);
  struct Command {std::string sCommand; std::string sParams; };
  std::vector<Command> mvQueuedCommands;
  
  // Member variables:
  bool mbUpdateLocalMap;
  boost::shared_ptr<KeyFrame> mpMostRecentKF;
  boost::shared_ptr<KeyFrame> mpNearestTrackerKF;
  std::vector<boost::shared_ptr<KeyFrame> > mvpKeyFrameQueue;  // Queue of keyframes from the tracker waiting to be processed
  std::vector<std::pair<boost::shared_ptr<KeyFrame>, boost::shared_ptr<MapPoint> > > mvFailureQueue; // Queue of failed observations to re-find
  std::vector<boost::shared_ptr<MapPoint> > mvNewQueue;   // Queue of newly-made map points to re-find in other KeyFrames
  
  double mdKeyFrameSpacing;  // Target metric distance between KeyFrames
  unsigned int mnMinMergeSize;
  unsigned int mnMinLocalKeyFrames;
  unsigned int mnTargetLocalKeyFrames;
  unsigned int mnTargetLocalPoints;
  double mdMaxSceneDepth;
  double mdMaxOverlapDistance;
  unsigned int mnRansacMinObservations;
  unsigned int mnRansacMaxHypotheses;
  unsigned int mnRansacBlocks;
  unsigned int mnRansacBlockSize;
  double mdMaxRotationError;
  double mdMaxTranslationError;
  int mnGlobalSE3;      // Should we use provided external position estimate (absolute measurements from GPS)
  int mnGlobalSO3;      // Should we use provided external attitude estimate (absolute measurements from compass, gyros etc.)
  GVars3::gvar3<double> mgvdKeyFrameSpacing;
  GVars3::gvar3<int> mgvnMinMergeSize;
  GVars3::gvar3<int> mgvnMinLocalKeyFrames;
  GVars3::gvar3<int> mgvnTargetLocalKeyFrames;
  GVars3::gvar3<int> mgvnTargetLocalPoints;
  GVars3::gvar3<double> mgvdMaxSceneAngle;
  GVars3::gvar3<double> mgvdMaxSceneDepth;
  GVars3::gvar3<int> mgvnRansacMinObservations;
  GVars3::gvar3<int> mgvnRansacMaxHypotheses;
  GVars3::gvar3<int> mgvnRansacBlocks;
  GVars3::gvar3<int> mgvnRansacBlockSize;
  GVars3::gvar3<double> mgvdMaxRotationError;
  GVars3::gvar3<double> mgvdMaxTranslationError;
  GVars3::gvar3<int> mgvnGlobalSE3;
  GVars3::gvar3<int> mgvnGlobalSO3;

  double mdLargestRadiusInImage;
  Vector<2> mv2ImageSize;
  
  int mnBundleNotConvergedCount;
  bool mbBundleConverged_Full;    // Has global bundle adjustment converged?
  bool mbBundleConverged_Recent;  // Has local bundle adjustment converged?
  
  // Thread interaction signalling stuff
  enum {COMPLETE, NEW_MAP, PREV_MAP} mResetType;
  bool mbResetRequested;   // A reset has been requested
  bool mbResetDone;        // The reset was done.
  bool mbBundleAbortRequested;      // We should stop bundle adjustment
  bool mbBundleRunning;             // Bundle adjustment is running
  bool mbBundleRunningIsRecent;     //    ... and it's a local bundle adjustment.
  bool mbMapMakerPaused;

  // Compute a mesh of the surface using adaptive compactly supported radial basis functions (ACSRBF)
  RBF* MultiScaleRBF(PointSet *ps, const float smooth, const float error);
  void ComputeMesh();

  std::ofstream ofsLogger;
};

#endif


















